import math
from car_op import send_command
from pose_get import get_robot_pose


def move_to(x_num, y_num):
    x0, y0, yaw = get_robot_pose()
    theta = calculate_angle_towards_y_axis(x0, y0, x_num, y_num)
    turn_to(theta)
    walk_to(x_num, y_num)
    return 0


def calculate_angle_towards_y_axis(x_a, y_a, x_b, y_b):
    # 计算向量AB
    dx = x_b - x_a
    dy = y_b - y_a

    # 计算向量AB与x轴正向的夹角（逆时针）
    angle_to_x_axis = math.atan2(dy, dx)

    # 将夹角转换为相对于y轴正向的夹角
    # 由于y轴正向与x轴正向垂直，所以夹角为90度（π/2弧度）
    angle_to_y_axis = math.pi / 2 - angle_to_x_axis

    # 确保角度在-pi到pi之间
    if angle_to_y_axis > math.pi:
        angle_to_y_axis -= 2 * math.pi
    elif angle_to_y_axis < -math.pi:
        angle_to_y_axis += 2 * math.pi

    return angle_to_y_axis


def move_by(method, x_num, y_num):
    x0, y0, yaw = get_robot_pose()
    if abs(x0 - x_num) > abs(y0 - y_num):
        #main_direct='x'
        while True:
            send_command(method, 50)
            x0, y0, yaw = get_robot_pose()
            if abs(y0 - y_num) >= 0.1:
                turn_to(calculate_angle_towards_y_axis(x0, y0, x_num, y_num))
            else:
                if abs(x0 - x_num) <= 0.1:
                    send_command('x', 0)
                    break
    else:
        #main_direct='y'

        while True:
            send_command(method, 50)
            x0, y0, yaw = get_robot_pose()
            if abs(x0 - x_num) >= 0.1:
                turn_to(calculate_angle_towards_y_axis(x0, y0, x_num, y_num))
            else:
                if abs(y0 - y_num) <= 0.1:
                    send_command('x', 0)
                    break


def move_by2(method, x_num, y_num, val):
    x0, y0, yaw = get_robot_pose()
    if abs(x0 - x_num) > abs(y0 - y_num):
        #main_direct='x'
        while True:
            send_command(method, val)
            x0, y0, yaw = get_robot_pose()
            if abs(y0 - y_num) >= 0.1:
                turn_to(calculate_angle_towards_y_axis(x0, y0, x_num, y_num))
            else:
                if abs(x0 - x_num) <= 0.1:
                    send_command('x', 0)
                    break
    else:
        #main_direct='y'

        while True:
            send_command(method, val)
            x0, y0, yaw = get_robot_pose()
            if abs(x0 - x_num) >= 0.1:
                turn_to(calculate_angle_towards_y_axis(x0, y0, x_num, y_num))
            else:
                if abs(y0 - y_num) <= 0.1:
                    send_command('x', 0)
                    break


def turn_to(angle):
    x0, y0, yaw = get_robot_pose()

    angle1, dire = get_delta_angle(yaw, angle)
    if dire == 'L':
        send_command('q', 10)
        while True:
            x0, y0, yaw = get_robot_pose()
            if abs(yaw - angle) < 0.08:
                send_command('x', 0)
                break
    if dire == 'R':
        send_command('e', 10)
        while True:
            x0, y0, yaw = get_robot_pose()
            if abs(yaw - angle) < 0.08:
                send_command('x', 0)
                break
    return 0


def walk_to(x_num, y_num, long):
    x0, y0, yaw = get_robot_pose()
    if abs(x0 - x_num) > abs(y0 - y_num):
        # main_direct='x'
        while True:
            send_command('w', 50)
            x0, y0, yaw = get_robot_pose()
            if abs(y0 - y_num) >= 0.1:
                turn_to(calculate_angle_towards_y_axis(x0, y0, x_num, y_num))
            else:
                if abs(x0 - x_num) <= 0.1:
                    send_command('x', 0)
                    break
    else:
        # main_direct='y'

        while True:
            send_command('w', 50)
            x0, y0, yaw = get_robot_pose()
            if abs(x0 - x_num) >= 0.1:
                turn_to(calculate_angle_towards_y_axis(x0, y0, x_num, y_num))
            else:
                if abs(y0 - y_num) <= 0.1:
                    send_command('x', 0)
                    break


def get_delta_angle(car_dir, target_dir):
    if car_dir < 0:
        car_dir += 2 * math.pi
    if target_dir < 0:
        target_dir += 2 * math.pi

    delta_angle = car_dir - target_dir

    if delta_angle < -math.pi:
        delta_angle += 2 * math.pi
    else:
        if delta_angle > math.pi:
            delta_angle -= 2 * math.pi
    if delta_angle > 0:
        direct = 'R'
    else:
        direct = 'L'
    return delta_angle, direct
